#ifndef TRAJECTORY_OPTIMIZATION_H
#define TRAJECTORY_OPTIMIZATION_H

#include <ros/ros.h>
#include "trajectory_optimization/trajectory_generator.h"
#include "trajectory_optimization/free_middle_waypoints_gen.h"
#include "nlopt.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <memory>

namespace Planner
{
    class TrajectoryOptimization
    {
    protected:
        TrajectoryGenerator traj_gen_;          // 用于生成初始条件
        TrajectoryGeneratorFree traj_gen_free_; // 用于迭代优化
        int order_;                             // 轨迹阶数
        int continus_;                          // 轨迹连续性
        int segments_;                          // 轨迹段数
        int type_;                              // 求解类型
        double speed_;                          // 预设速度
        double vmax_;                           // 最大速度
        double amax_;                           // 最大加速度
        int algorithm_;                         // 非线性优化使用的算法
        double opt_time_;                       // 优化时间
        int opt_iteration_;                     // 迭代次数
        int iter_num_;                          // 当前迭代次数
        double opt_step_;                       // 初始迭代步长
        double resolution_;                     // 空间分辨率
        double time_gap_;                       // 时间分辨率
        double secure_dis_;                     // 安全距离
        double pos_tol_;                        // 中间路径点允许移动的距离
        bool print_info_;                       // 输出设置
        Vector3d startVel_;                     // 边界条件
        Vector3d startAcc_;
        Vector3d goalVel_;
        Vector3d goalAcc_;
        double wt_, ws_, wc_, wdv_, wda_;                // 代价函数权重
        double alphac_, alphav_, alphaa_, rc_, rv_, ra_; // 指数惩罚项系数
        MatrixXd R_, Rfp_, Rpp_, AiMC_;                  // 用于计算平滑项梯度
        vector<MatrixXd> dp_, df_;
        vector<double> time_seq_;     // 分段轨迹时间戳
        vector<Vector3d> way_points_; // 路径点
        vector<MatrixXd> coeffs_;     // 轨迹系数
        vector<double> cost_val_;     // 代价函数

        // 好像只传kdtree进来就够了，cloud_map_也先传着吧，反正是指针，不太占资源
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_map_;
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_;
        std::shared_ptr<nlopt::opt> nlopt_;

        static double costFunc(const vector<double> &x, vector<double> &grad, void *func_data);

        double getTimeCostFunc();

        double getDistance(double x, double y, double z);

        double getDistanceAndGrad(double x, double y, double z, Vector3d &grad);

        double getCollisionPenaltyByLoc(State &loc);

        double getVelPenaltyByLoc(State &loc);

        double getAccPenaltyByLoc(State &loc);

        void getCollisionCostAndDynamicCostFunc(double &cost_collision, double &cost_dynamic_vel, double &cost_dynamic_acc);

        double getCostFunc(const vector<double> &x);

    public:
        TrajectoryOptimization(){};
        ~TrajectoryOptimization(){};

        void setParams(const ros::NodeHandle &nh);

        void setWayPoints(const vector<Vector3d> &way_pts);

        void setBoundaryCondition(const Vector3d &startVel, const Vector3d &startAcc,
                                  const Vector3d &goalVel, const Vector3d &goalAcc);

        // void setCloudMap(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::KdTreeFLANN<pcl::PointXYZ> &tree);
        void setCloudMap(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr &tree);

        int optimize();

        void getCoeffs(vector<MatrixXd> &coeffs) { traj_gen_free_.getCoeffs(coeffs); };

        void getTimeSeq(vector<double> &time_seq) { time_seq = time_seq_; };

        void getCostVal(vector<double> &cost_val) { cost_val = cost_val_; };
    };

} // namespace Planner

#endif